/*
 * @Author: your name
 * @Date: 2020-05-16 10:42:17
 * @LastEditTime: 2020-05-16 18:17:47
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \app\xp\driver\syntron\brush_s.c
 */ 
#include "brush_s.h"

BRUSH_state brush_state;
static int g_brush[brush_speedmax] ={0,300,600,BRUSH_RATED_SPEED};

/**
 * @description: brush driver configure some parameters
 * @param {type} void
 * @return: void
 */
void xp_brush_config(void)
{
    brush_state.init = 0;
    brush_state.run = brush_run_no;
    brush_state.dir = brush_dir_no;
    brush_state.exp = brush_exp_1;
    brush_state.acc_dcctime = ACC_DCC_TIME;
    brush_state.speed_grade = brush_speed_master;
    brush_state.res1[0]=0;
    brush_state.res1[1]=0;
    brush_state.torque_rate = 0;
    brush_state.brush_syn_hander = &synro_brush_handler;
    brush_state.brush_syn_hander->config.acc_time = ACC_TIME;
    brush_state.brush_syn_hander->config.acc_time = DCC_TIME;
    brush_state.brush_syn_hander->config.drivemode = syn_speed;
    brush_state.brush_syn_hander->config.direct = 1;
}

/**
 * @description: Set the acceleration time of the brush motor
 * @param time_ms unit ms
 * @return: 
 *         0 success
 *         -1 fail
 */
int xp_brush_set_motors_acc_timems(u16 time_ms)
{
    int ret = 0;
    ret = synron_write_register(brush_state.brush_syn_hander,PARAMETER_TIME_ACCELERATE,time_ms);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_wheel_set_motors_acc_timems A is %d\n",time_ms);  
        xp_io_write_pin(B_BOARD_ID,B_INPUT_PIN,B_HIGH_STATE);//通信有问题紧急输入制动信号
        return SYNRON_COMMUN_ERR;           
    }
    return SYNRON_SUCCESS;
}

/**
 * @description: Set the deceleration  time of the brush motor
 * @param time_ms unit ms
 * @return: 
 *         0 success
 *         -1 fail
 */
int xp_brush_set_motors_dcc_timems(u16 time_ms)
{
    int ret = 0;
    ret = synron_write_register(brush_state.brush_syn_hander,PARAMETER_TIME_DECELERATION,time_ms);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_wheel_set_motors_acc_timems A is %d\n",time_ms);  
        xp_io_write_pin(B_BOARD_ID,B_INPUT_PIN,B_HIGH_STATE);//通信有问题紧急输入制动信号
        return SYNRON_COMMUN_ERR;           
    }
    return SYNRON_SUCCESS;
}


/**
 * @description: Set the deceleration and acceleration time of the brush motor
 * @param time_ms unit ms
 * @return: 
 *          0 success
 *          -1 /-2  fail
 */
int xp_brush_set_acc_dcc_times(u16 time_ms)
{
    int ret =0;
    ret = xp_brush_set_motors_dcc_timems(time_ms);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("set brush dcc times fail");
        return SYNRON_NOR_ERR;
    }
    ret = xp_brush_set_motors_acc_timems(time_ms);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("set brush acc times fail");
        return SYNRON_NOR_ERR;
    }
    return SYNRON_SUCCESS;
}

/**
 * @description: brush initialization
 * @param :prot : uart port
 *         speed: baundspeed (default 115200)
 *         slaveid : slave adress
 * @return:  0 success
 *           -1 / -2  fail
 */
int xp_brush_init(u8 port,int speed,u8 slaveid)
{   
    int ret = 0;
    xp_brush_config();
    if(brush_state.init == 1)
    {
        LOG_SYNRON("the xp brush drive hava init");
        return 1;
    }
    ret = synron_hander_init(brush_state.brush_syn_hander,1,slaveid,port,speed);
    if(ret != SYNRON_SUCCESS)
    {
       LOG_SYNRON(" synron_hander_init brush_state.brush_syn_hander fail !!");
        return SYNRON_NOR_ERR;
    }
    ret = xp_synron_speed_init(brush_state.brush_syn_hander);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_synron_speed_init(brush_state.brush_syn_hander fail !!");
        return SYNRON_NOR_ERR;
    }
    
    /*
    线没接先注释掉
    ret = synron_setemg_pinnum(brush_state.brush_syn_hander,B_IPUT_DRIVE_EMG_PIN);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("synron_setemg_pinnum brush_state hander fail !!");
        return SYNRON_NOR_ERR;
    }
    */
    ret = xp_brush_set_acc_dcc_times(brush_state.acc_dcctime);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_brush_set_acc_dcc_times brush_state hander fail !!");
        return SYNRON_NOR_ERR;
    }
    brush_state.init = 1;
    return SYNRON_SUCCESS;
}

/**
 * @description: make brush motor stoped 
 * @param {type} void 
 * @return: 
 *          0 success
 *          -1 fail
 */
int xp_brush_stop()
{
    int ret = 0; 
    ret = synron_speedmode_motorstop(brush_state.brush_syn_hander);
    if(ret != SYNRON_SUCCESS)
    {
        xp_io_write_pin(B_BOARD_ID,B_INPUT_PIN,B_HIGH_STATE);//通信有问题紧急输入制动信号
        LOG_SYNRON("synron_speedmode_motorstop brush stoped !!");
        return SYNRON_COMMUN_ERR;
    }
    brush_state.dir = brush_dir_no;
    return SYNRON_SUCCESS;
}

/**
 * @description: xp_brush_read_fluent_feedback
 * @param {type} void 
 * @return: 
 *          0 success
 *          -1 fail
 */
int xp_brush_read_fluent_feedback()
{
    int ret = 0; 
    ret = synron_get_validtor_rate_value(brush_state.brush_syn_hander);
    if(ret == READADD_16_ERR)
    {
        LOG_SYNRON("synron_get_validtor_rate_value error!!");
        return SYNRON_COMMUN_ERR;
    }
    brush_state.torque_rate = ret;
    return ret;
}


/**
 * @description: xp_brush_rotate 
 * @param  dir : direaction
 * @param  grade : speed grade see g_brush array
 * @return: 
 *          0 success
 *          -1 fail
 */
int xp_brush_rotate(u8 dir ,u8 grade)
{
    int ret = 0;
    if(grade > brush_speedmax)
    {
        LOG_SYNRON("grade value is wrong");
        return SYNRON_NOR_ERR;
    }
    if(dir == brush_dir_a)
    {
        brush_state.dir =brush_dir_a;
        ret = synron_speedmode_setvalue(brush_state.brush_syn_hander,g_brush[grade]);
        if(ret != SYNRON_SUCCESS)
        {
            LOG_SYNRON("synron_speedmode_setvalue fail!");
            xp_io_write_pin(B_BOARD_ID,B_INPUT_PIN,B_HIGH_STATE);//通信有问题紧急输入制动信号
            return SYNRON_COMMUN_ERR;
        }
    }
    if(dir == brush_dir_b)
    {
        brush_state.dir =brush_dir_b;
        ret = synron_speedmode_setvalue(brush_state.brush_syn_hander,-g_brush[grade]);
        if(ret != SYNRON_SUCCESS)
        {
            LOG_SYNRON("synron_speedmode_setvalue fail!");
            xp_io_write_pin(B_BOARD_ID,B_INPUT_PIN,B_HIGH_STATE);//通信有问题紧急输入制动信号
            return SYNRON_COMMUN_ERR;
        }
    }
    if(dir == brush_dir_no)
    {
        brush_state.dir =brush_dir_no;
        ret = xp_brush_stop();
        if(ret != SYNRON_SUCCESS)
        {
            LOG_SYNRON("xp_brush_stop fail!");
            xp_io_write_pin(B_BOARD_ID,B_INPUT_PIN,B_HIGH_STATE);//通信有问题紧急输入制动信号
            return SYNRON_COMMUN_ERR;
        }
    }
    if((dir == brush_dir_change)&&(brush_state.dir != brush_dir_no))
    {
        if(brush_state.dir == brush_dir_b)
        {
            brush_state.dir = brush_dir_a;
            ret = synron_speedmode_setvalue(brush_state.brush_syn_hander,g_brush[grade]);
            if(ret != SYNRON_SUCCESS)
            {
                LOG_SYNRON("synron_speedmode_setvalue fail!");
                xp_io_write_pin(B_BOARD_ID,B_INPUT_PIN,B_HIGH_STATE);//通信有问题紧急输入制动信号
                return SYNRON_COMMUN_ERR;
            }
        }
         if(brush_state.dir == brush_dir_a)
        {
            brush_state.dir = brush_dir_b;
            ret = synron_speedmode_setvalue(brush_state.brush_syn_hander,-g_brush[grade]);
            if(ret != SYNRON_SUCCESS)
            {
                LOG_SYNRON("synron_speedmode_setvalue fail!");
                xp_io_write_pin(B_BOARD_ID,B_INPUT_PIN,B_HIGH_STATE);//通信有问题紧急输入制动信号
                return SYNRON_COMMUN_ERR;
            }
        }
        
    }
    return SYNRON_SUCCESS;
}

/*
* @description: xp_io_test_verify
* @param type:debug type
* @param fun:function
* @param param:     
* @return: 
*/

void xp_io_test_verify()
{
    u8 io_num = 0;
    for(io_num =0;io_num<18;io_num++)
    {
        xp_io_write_pin(1,io_num,0);
    }
    aos_msleep(1000);
    for(io_num =0;io_num<18;io_num++)
    {
        xp_io_write_pin(1,io_num,1);
    }
    aos_msleep(2000);
    for(io_num =0;io_num<18;io_num++)
    {
        xp_io_write_pin(1,io_num,0);
        aos_msleep(1000);
        xp_io_write_pin(1,io_num,1);
    }
    aos_reboot();
}


/*
* @description: xp_brush_debug
* @param type:debug type
* @param fun:function
* @param param:type: 
*              fun function 
*              param     
* @return: 
*/
int xp_brush_debug(char *type,char *fun,char *param)
{

    u8 dir = 0;
    u8 grade = 0;
    int ret = 0;
    u16 time_ms = 0;
    if(type==NULL)
    {
        LOG_SYNRON("type  is null");
        return 0;
    }
    if(strcmp(type,"brush") == 0)
    {
        if(strcmp(fun,"init") == 0)
        {
            LOG_SYNRON("start brush init !");
            ret = xp_brush_init(1,115200,SYNRON_BRUSH_SLAVEID);  //debug 模式 SYNRON_A_MOTOR_SLAVEID 临时代替
            if(ret != SYNRON_SUCCESS)
            {
                LOG_SYNRON("brush init fail!");
                return 1;
            }
            LOG_SYNRON("brush init success !!!");

        }
        else if(strcmp(fun,"stop") == 0)
        {
            LOG_SYNRON(" brush will stop !!");
            ret = xp_brush_stop();
            if(ret != SYNRON_SUCCESS)
            {
                LOG_SYNRON("brush stop fail!");
                return 1;
            }
        }
        else if(strcmp(fun,"feedback") == 0)
        {
            LOG_SYNRON(" brush torque_rate feedback !!");
            ret = xp_brush_read_fluent_feedback();
            if(ret != SYNRON_SUCCESS)
            {
                LOG_SYNRON("brush torque_rate is %d  %!",ret);
                return 1;
            }
        }
        else if(strcmp(fun,"accdcc") == 0)
        {
            time_ms = strtoul(param, NULL, 10);
            LOG_SYNRON("brush set accdcc times %d ms",time_ms);
            ret = xp_brush_set_acc_dcc_times(time_ms);
            if(ret != SYNRON_SUCCESS)
            {
                LOG_SYNRON("brush set acc and dcc time fail");   
                return 1; 
            }
            LOG_SYNRON("brush set acc and dcc times success");
        }
        else
        {
            return 0;
        }
        return 1;
    }
    if(strcmp(type,"brush_d_g") == 0)
    {
         dir = strtoul(fun, NULL, 10);
         grade = strtoul(param, NULL, 10);
         LOG_SYNRON("set brush dir is %d  and speed is %d  grade is %d",dir,g_brush[grade],grade);
         ret = xp_brush_rotate(dir,grade);
         if(ret != SYNRON_SUCCESS)
         {
            LOG_SYNRON("xp brush set speed fail ");
            return 1;
         } 
         LOG_SYNRON("set brush  dir and speed succcess");
         return 1;
    }
    if(strcmp(type,"iotest")==0){
        println("xp io test");
        xp_io_test_verify();
        return 1;
    }
    else
    {
        return 0;
    }
    
}


//endfile
